clear;close all;clc;
dbstop if error;

Pro_width = 1280;
Pro_height = 720;


%%
N = 20;T = 20;Plus =2.98451302091030;
f = Pro_width / T;
n=log2(f);%互补张数
numImages = 20;%处理的图片组数
data = '20251009';
mkdir("phase\"+data);%创建文件夹
folder="Pic\"+data+"\calibration\";

width = 2048;
height = 1536;
% width = 1440;
% height = 1080;

H1K = load("H1K.mat").H1K;
H2K = load("H2K.mat").H2K;

V1K = load("V1K.mat").V1K;
V2K = load("V2K.mat").V2K;


% for i=1:numImages
%     file_old = "OldPic\"+data+"\"+num2str(i);
%     file_new = "Pic\"+data+"\"+num2str(i);
%     Rename(string(file_old),55,string(file_new));
% end


%%
%生成高斯滤波
%GaussianFilter = fspecial('gaussian', [9,9], 3);%最后一个参数为sigma
figure;
for index = 1:numImages
    tic
    %计算横条纹包裹相位
    H_I_sin = zeros(height, width);
    H_I_cos = zeros(height, width);
    H_I_B = zeros(height, width);
    H_I_back = zeros(height, width);
 disp("H二值相移图");
    for k = 1: N
            % 计算当前相位移动的值
            shiftVal = (2 * pi / N) * (k-1);% 普通相移值
            % 根据相位值计算当前像素的强度
            I = im2double(imread(folder+num2str(index)+"\"+num2str((n+1)+(k))+".bmp"));
            disp(folder+num2str(index)+"\"+num2str((n+1)+(k))+".bmp");
            H_I_sin = H_I_sin + I .* sin(shiftVal);
            H_I_cos = H_I_cos + I .* cos(shiftVal);
            H_I_back=H_I_back+I.*255;
    end
    H_phase = -atan2(H_I_sin, H_I_cos); % 计算截断相位
    H_I_B = 2.0/N.*sqrt(H_I_sin.^2+H_I_cos.^2).*255;
    H_I_back=H_I_back./N;
    H_ShadowMask=H_I_B>15;
    H_ShadowMask = bwareaopen(H_ShadowMask,100);
%%%%%%%%%%%%%%%%%%%%%%%%
        H_BinarybitPlane = cell(n+1, 1);
        H_FloatBinarybitPlane = cell(n+1, 1);
        
        H_powersV2 = zeros(1, n+1);
        H_powersV1 = zeros(1, n);
        
        H_powersV2(1) = 2^n;
        for k = 2:(n+1)
            H_powersV2(k) = H_powersV2(k-1) / 2.0;
        end
        
        for k = 1:n
            H_powersV1(k) = H_powersV2(k+1);
        end
        
        for k = 1:(n+1)
            I = imread(folder+num2str(index)+"\"+num2str(k)+".bmp");
            if k <= n
                disp(folder+num2str(index)+"\"+num2str(k)+".bmp");
            else
                disp("H互补格雷码");
                disp(folder+num2str(index)+"\"+num2str(k)+".bmp");
                imshow((I>H_I_back).*H_ShadowMask);
            end
            
            BinaryI = (I > H_I_back) .* H_ShadowMask;
            H_BinarybitPlane{k} = BinaryI;
            
            if k > 1
                H_BinarybitPlane{k} = xor(H_BinarybitPlane{k-1}, H_BinarybitPlane{k});
            end
            
            H_FloatBinarybitPlane{k} = double(H_BinarybitPlane{k});
        end
        
        HK1 = zeros(height, width);
        HK2 = zeros(height, width);
        
        for k = 1:(n+1)
            if k <= n
                HK1 = HK1 + H_FloatBinarybitPlane{k} * H_powersV1(k);
            end
            HK2 = HK2 + H_FloatBinarybitPlane{k} * H_powersV2(k);
        end
        
        HK2 = floor((HK2 + 1.0) / 2.0);

    H_mask1 = double(H_phase <= -pi / 2.);
    H_mask2 = double((-pi / 2 < H_phase) & (H_phase <= pi / 2.));
    H_mask3 = double(H_phase >  pi / 2.);
    PHDeformed_Unwrap = (H_phase + 2. * pi * HK2) .* H_mask1 + (H_phase + 2. * pi * HK1) .* H_mask2 + (H_phase + 2. * pi * HK2 - 2. * pi) .* H_mask3;
    PHDeformed_Unwrap=PHDeformed_Unwrap+Plus;       
    % PHDeformed_Unwrap(H_ShadowMask==0) = nan;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




    %计算横条纹包裹相位
    V_I_sin = zeros(height, width);
    V_I_cos = zeros(height, width);
    V_I_B = zeros(height, width);
    V_I_back = zeros(height, width);
disp("V二值相移图");
    for k = 1: N
            % 计算当前相位移动的值
            shiftVal = (2 * pi / N) * (k-1);% 普通相移值
            % 根据相位值计算当前像素的强度
            I = im2double(imread(folder+num2str(index)+"\"+num2str((n+1)+N+(n+1)+(k))+".bmp"));
            disp(folder+num2str(index)+"\"+num2str((n+1)+N+(n+1)+(k))+".bmp");
            V_I_sin = V_I_sin + I .* sin(shiftVal);
            V_I_cos = V_I_cos + I .* cos(shiftVal);
            V_I_back=V_I_back+I.*255;
    end
    V_phase = -atan2(V_I_sin, V_I_cos); % 计算截断相位
    V_I_B = 2.0/N.*sqrt(V_I_sin.^2+V_I_cos.^2).*255;
    V_I_back=V_I_back./N;
    V_ShadowMask=V_I_B>15;
    V_ShadowMask = bwareaopen(V_ShadowMask,100);
     

    V_BinarybitPlane = cell(n+1, 1);
    V_FloatBinarybitPlane = cell(n+1, 1);
    
    V_powersV2 = zeros(1, n+1);
    V_powersV1 = zeros(1, n);
    
    V_powersV2(1) = 2^n;
    for k = 2:(n+1)
        V_powersV2(k) = V_powersV2(k-1) / 2.0;
    end
    
    for k = 1:n
        V_powersV1(k) = V_powersV2(k+1);
    end
    
    for k = 1:(n+1)
        I = imread(folder+num2str(index)+"\"+num2str((n+1)+N+k)+".bmp");
        if k <= n
            disp(folder+num2str(index)+"\"+num2str((n+1)+N+k)+".bmp");
        else
            disp("V互补格雷码");
            disp(folder+num2str(index)+"\"+num2str((n+1)+N+k)+".bmp");
            imshow((I>V_I_back).*V_ShadowMask);
        end
        
        BinaryI = (I > V_I_back) .* V_ShadowMask;
        V_BinarybitPlane{k} = BinaryI;
        
        if k > 1
            V_BinarybitPlane{k} = xor(V_BinarybitPlane{k-1}, V_BinarybitPlane{k});
        end
        
        V_FloatBinarybitPlane{k} = double(V_BinarybitPlane{k});
    end
    
    VK1 = zeros(height, width);
    VK2 = zeros(height, width);
    
    for k = 1:(n+1)
        if k <= n
            VK1 = VK1 + V_FloatBinarybitPlane{k} * V_powersV1(k);
        end
        VK2 = VK2 + V_FloatBinarybitPlane{k} * V_powersV2(k);
    end
    
    VK2 = floor((VK2 + 1.0) / 2.0);

%%

    Vmask1 = double(V_phase <= -pi / 2.);
    Vmask2 = double((-pi / 2 < V_phase) & (V_phase <= pi / 2.));
    Vmask3 = double(V_phase >  pi / 2.);
    PVDeformed_Unwrap = (V_phase + 2. * pi * VK2) .* Vmask1 + (V_phase + 2. * pi * VK1) .* Vmask2 + (V_phase + 2. * pi * VK2 - 2. * pi) .* Vmask3;
    PVDeformed_Unwrap=PVDeformed_Unwrap+Plus;
    % PVDeformed_Unwrap(PVDeformed_Unwrap==0) = nan;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
        toc
        % figure;
        % imshow(PHDeformed_Unwrap,[]);
        % figure;
        % imshow(PVDeformed_Unwrap,[]);

        imwrite(mat2gray(PHDeformed_Unwrap), "phase\"+data+"\hPhases_"+num2str(index)+".bmp");
        imwrite(mat2gray(PVDeformed_Unwrap), "phase\"+data+"\vPhases_"+num2str(index)+".bmp");
        save("phase\"+data+"\hPhases_"+ num2str(index)+".mat", 'PHDeformed_Unwrap');
        save("phase\"+data+"\vPhases_"+num2str(index)+".mat", 'PVDeformed_Unwrap');
end